/*INCLUDES *******************************************************************/
#include "ses_timer.h"
#include "ses_scheduler.h"
#include <stdio.h>

/* TYPES *********************************************************************/
typedef struct {
	task_t task;
	uint16_t expire;
} taskDescriptor_t;

/* PRIVATE VARIABLES *********************************************************/
volatile taskDescriptor_t tasks[SCHEDULER_ENTRIES];

/*FUNCTION DEFINITION ********************************************************/

void scheduler_update(void) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		cli();
		if (tasks[i].expire != 0) {
			--tasks[i].expire;
		}
		sei();
	}
}

void scheduler_init() {

	// init scheduler w/ NULL tasks
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		cli();
		taskDescriptor_t t = {.task = NULL, .expire = 0};
		tasks[i] = t;
		sei();
	}

	// start timer2 and begin updating each 1 ms
	timer2_init(scheduler_update);
	sei();
}

void scheduler_run() {
	task_t task ;
	while (1) {
		for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
			cli();
			if (tasks[i].expire == 0 && tasks[i].task != NULL) {
				task = tasks[i].task;
				tasks[i].task = NULL;
				tasks[i].expire = 0;
				sei();
				task();
			}
			sei();
		}
	}
}

bool scheduler_add(task_t task, uint16_t expire) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		cli();
		if (tasks[i].task == NULL) {
			tasks[i].task = task;
			tasks[i].expire = expire;
			return true;
		}
		sei();
	}
	return false;
}

void scheduler_remove(task_t task) {
	for (int i = 0; i < SCHEDULER_ENTRIES; i++) {
		cli();
		if (tasks[i].task == task){
			tasks[i].task = NULL;
		}
		sei();
	}
}
